#include "feature_extraction.h"
#include <message_filters/sync_policies/approximate_time.h>
#include <radar_detector/radar_boundsConfig.h>
#include <dynamic_reconfigure/server.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv/cv.hpp>
#include <opencv2/calib3d.hpp>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

int8_t flag;
my_calib::radar_boundsConfig bounds;
ros::Publisher pattern_publisher;
ros::Publisher image_pattern_publisher;
void flag_cb(const std_msgs::Int8::ConstPtr &msg)
{
    flag = msg->data; // read flag published by input_sample node
    ROS_INFO("flag recv");
}
int out_pos(double cam_x, double cam_y, double radar_x, double radar_y, double radar_z)
{
    ros::Time time = ros::Time::now();
    // std::string filename =  std::to_string(time.toSec())+".txt";
    std::string filename = "text.txt";
    std::ofstream ostrm(filename, std::ios_base::app);
    ostrm << cam_x << " " << cam_y << " " << radar_x << " " << radar_y << " " << radar_z << '\n'; // text output
    ROS_INFO("write current points group");
}
int image_feature_extraction(cv::Mat image, double *x, double *y)
{
    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
    cv::Size2i patternNum(6, 4);
    cv::Size2i patternSize(100, 100);

    cv::Mat corner_vectors = cv::Mat::eye(3, 5, CV_64F);
    cv::Mat chessboard_normal = cv::Mat(1, 3, CV_64F);
    // checkerboard corners, middle square corners, board corners and centre
    std::vector<cv::Point2f> image_points, imagePoints1, imagePoints;
    cv::Mat gray;
    std::vector<cv::Point2f> corners, corners_undistorted;
    std::vector<cv::Point3f> grid3dpoint;
    cv::cvtColor(image, gray, CV_BGR2GRAY);

    // Find checkerboard pattern in the image
    bool patternfound = cv::findChessboardCorners(gray, patternNum, corners,
                                                  cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);

    if (patternfound)
    {
        // Find corner points with sub-pixel accuracy
        cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
        cv::Size imgsize;
        imgsize.height = image.rows;
        imgsize.width = image.cols;

        // Board corners w.r.t board frame
        for (int i = 0; i < patternNum.height; i++)
        {
            for (int j = 0; j < patternNum.width; j++)
            {
                cv::Point3f tmpgrid3dpoint;
                // Translating origin from bottom left corner to the centre of the checkerboard
                tmpgrid3dpoint.x = i * patternSize.height;
                tmpgrid3dpoint.y = j * patternSize.width;
                tmpgrid3dpoint.z = 0;
                grid3dpoint.push_back(tmpgrid3dpoint);
            }
        }

        cv::Mat rvec(3, 3, cv::DataType<double>::type); // Initialization for pinhole and fisheye cameras
        cv::Mat tvec(3, 1, cv::DataType<double>::type);

        cv::solvePnP(grid3dpoint, corners, i_params.cameramat, i_params.distcoeff, rvec, tvec);
        std::cout << rvec << std::endl;
        //  std::cout<<tvec<<std::endl;
        //  std::cout<< i_params.cameramat<<std::endl;
        //   std::cout<< i_params.distcoeff<<std::endl;
        //   std::cout<< grid3dpoint<<std::endl;
        // project points
        cv::Point3f tmpgrid3dpoint;
        tmpgrid3dpoint.x = -10;
        tmpgrid3dpoint.y = 490;
        tmpgrid3dpoint.z = 30;
        grid3dpoint.push_back(tmpgrid3dpoint);
        cv::projectPoints(grid3dpoint, rvec, tvec, i_params.cameramat, i_params.distcoeff, image_points);
        *x = image_points[image_points.size() - 1].x;
        *y = image_points[image_points.size() - 1].y;
        for (int i = 0; i < grid3dpoint.size(); i++)
        {
            if (i == grid3dpoint.size() - 1)
            {
                cv::circle(image, image_points[i], 7, CV_RGB(255, 255, 0), 2);

                continue;
            }
            cv::circle(image, image_points[i], 5, CV_RGB(255, 0, 0), 2);
            // ROS_INFO("draw circle %d",i);
            // std::cout<<image_points[i]<<std::endl;
            // ROS_INFO("draw circle %f",i);
        }

        // Extrinsic parameter: Transform Velodyne -> cameras
        // tf::Matrix3x3 rot;

        // rot.setRPY(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2));

        // Eigen::MatrixXf t1(4,4),t2(4,4);
        // t1 << rot.getRow(0)[0], rot.getRow(0)[1], rot.getRow(0)[2], rvec.at<double>(0),
        //     rot.getRow(1)[0], rot.getRow(1)[1], rot.getRow(1)[2], rvec.at<double>(1),
        //     rot.getRow(2)[0], rot.getRow(2)[1], rot.getRow(2)[2], rvec.at<double>(2),
        //     0, 0, 0, 1;
        // t2 = t1.inverse();
        // Eigen::Affine3f transform_A = Eigen::Affine3f::Identity();
        // transform_A.matrix() << t2(0,0), t2(0,1), t2(0,2), t2(0,3),
        //     t2(1,0), t2(1,1), t2(1,2), t2(1,3),
        //     t2(2,0), t2(2,1), t2(2,2), t2(2,3),
        //     t2(3,0), t2(3,1), t2(3,2), t2(3,3);

        // std::cout<<transform_A.matrix()<<std::endl;
        // Eigen::Vector3d p1 = Eigen::Vector3d(-0.01,0.49,0.03);
        // Eigen::Vector3d p2;
        // p2 = transform_A*p1;

        // Need change
        // chessboardpose is a 3*4 transform matrix that transforms points in board frame to camera frame | R&T
        cv::Mat chessboardpose = cv::Mat::eye(4, 4, CV_64F);
        cv::Mat tmprmat = cv::Mat(3, 3, CV_64F); // rotation matrix
        cv::Rodrigues(rvec, tmprmat);            // Euler angles to rotation matrix

        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                chessboardpose.at<double>(j, k) = tmprmat.at<double>(j, k);
            }
            chessboardpose.at<double>(j, 3) = tvec.at<double>(j);
        }
        cv_ptr->image = image;
        cv_ptr->header.frame_id = "cam";
        // Publish the image projection
        ros::Time time = ros::Time::now();

        cv_ptr->encoding = "bgr8";
        cv_ptr->header.stamp = time;
        image_pattern_publisher.publish(cv_ptr->toImageMsg());
    }
}

int radarpoints_feature_extraction(pcl::PointCloud<pcl::PointXYZDR>::Ptr cloud, pcl::PointCloud<pcl::PointXYZDR>::Ptr points, double *x, double *y, double *z)
{

    pcl::PointXYZDR point;
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {

        float x = cloud->points[i].x;
        float y = cloud->points[i].y;
        float z = cloud->points[i].z;

        float range = sqrt(x * x + y * y + z * z); // compute range using x ,y and z
        float rcs = cloud->points[i].rcs;          // RCS value

        if (x < bounds.x_max && y < bounds.y_max && z < bounds.z_max &&
            x > bounds.x_min && y > bounds.y_min && z > bounds.z_min &&
            rcs < bounds.rcs_max && rcs > bounds.rcs_min)
        {
            point.x = x;
            point.y = y;
            point.z = z;
            point.rcs = rcs;
            point.doppler_v = cloud->points[i].doppler_v;

            // std::cout << "push_back" << i << "\t" << rcs << std::endl;
            points->push_back(point);
        }
    }
    if (points->size() == 1)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

int callback(const sensor_msgs::Image::ConstPtr &image, const sensor_msgs::PointCloud2::ConstPtr &pc)
{
    double image_x = 0;
    double image_y = 0;
    double radar_x = 0;
    double radar_y = 0;
    double radar_z = 0;

    // 点云数据处理
    pcl::PointCloud<pcl::PointXYZDR>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZDR>);
    pcl::PointCloud<pcl::PointXYZDR>::Ptr cloud_ret(new pcl::PointCloud<pcl::PointXYZDR>);
    sensor_msgs::PointCloud2::Ptr sensor_cloud(new sensor_msgs::PointCloud2);
    pcl::fromROSMsg(*pc, *cloud1);
    int ret = 0;

    ret = radarpoints_feature_extraction(cloud1, cloud_ret, &radar_x, &radar_y, &radar_z);
    pcl::toROSMsg(*cloud_ret, *sensor_cloud);
    sensor_cloud->header.frame_id = "ars548";
    pattern_publisher.publish(*sensor_cloud);

    // std::cout << cloud_ret->points.size() << std::endl;

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }

    image_feature_extraction(cv_ptr->image, &image_x, &image_y);

    if (flag == 1)
    {
        flag = 0;
        if (cloud_ret->points.size() == 1)
        {
            out_pos(image_x, image_y, 0, 0, 0);
            ROS_INFO("One point");
        }
    }
    return 0;
}
// bounds_callback
int bounds_callback(my_calib::radar_boundsConfig &config, uint32_t level)
{
    bounds = config;
    ROS_INFO("Reconfigure Request: %lf %lf %lf %lf %lf %lf",
             config.x_min, config.x_max, config.y_min, config.y_max, config.z_min, config.z_max);
}

int read_parameter()
{
    std::string pkg_loc;
    // Read input parameters from configuration file
    pkg_loc = ros::package::getPath("cr_optimizer");
    std::ifstream infile(pkg_loc + "/cfg/initial_params.txt");
    int cb_l, cb_b, l, b, e_l, e_b;
    infile >> i_params.camera_topic;
    infile >> i_params.radar_topic;

    infile >> cb_l;
    infile >> cb_b;
    i_params.grid_size = std::make_pair(cb_l, cb_b);

    infile >> i_params.square_length;

    infile >> l;
    infile >> b;
    i_params.board_dimension = std::make_pair(l, b);

    double camera_mat[9];
    for (int i = 0; i < 9; i++)
    {
        infile >> camera_mat[i];
    }
    cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(i_params.cameramat);
    double dist_coeff[5];
    for (int i = 0; i < 5; i++)
    {
        infile >> dist_coeff[i];
    }
    cv::Mat(1, 5, CV_64F, &dist_coeff).copyTo(i_params.distcoeff);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_extraction");
    ros::NodeHandle nh("~");

    // Subscriber
    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/pylon_camera_node/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> radar_cloudpoints_sub(nh, "/ars548/cloudpoints", 1);
    //    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_sub, radar_cloudpoints_sub, 10);
    //    sync.registerCallback(boost::bind(&callback, _1, _2));
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
    // Synchronizer to get synchronized camera-lidar scan pairs
    //    message_filters::Synchronizer<MySyncPolicy> sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(5), image_sub, radar_cloudpoints_sub);
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, radar_cloudpoints_sub);

    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::Subscriber flag_subscriber;
    flag_subscriber = nh.subscribe<std_msgs::Int8>("/flag", 1, &flag_cb);

    // Publisher

    pattern_publisher = nh.advertise<sensor_msgs::PointCloud2>("radar_pattern", 10);
    // marker_publisher_  = nh_.advertise<visualization_msgs::Marker>("radar_marker", 10);
    image_pattern_publisher = nh.advertise<sensor_msgs::Image>("image_pattern", 10);
    // Dynamic reconfigure gui to set the experimental region bounds
    dynamic_reconfigure::Server<my_calib::radar_boundsConfig> server;
    dynamic_reconfigure::Server<my_calib::radar_boundsConfig>::CallbackType f;
    f = boost::bind(&bounds_callback, _1, _2);
    server.setCallback(f);

    ROS_INFO("READ PARAMETERS");
    read_parameter();

    // out_pos();
    ros::spin();

    return 0;
}
